/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#include "JPEGEncoder.hpp"
#include "TOccupancyGrid.h"
#include "glutImageGUI.h"
#include "CJanRoutePlanner.h"
#include <unistd.h>

void draw(TOccupancyGrid &O, unsigned char *rgb, int Nx, int Ny){
	O.fillRGB(rgb,1.0);
	displayImage(0, rgb,Ny,Nx);
	usleep(500*1000);
}

void testPrimitives(TOccupancyGrid &O, unsigned char *rgb, int Nx, int Ny){
	grid_t empty=0.0;
	grid_t rigid=254;
	grid_t occu=128;
	
	/// Set some primitives
	fprintf(stderr,"circle (0,0) radius 3\n");
	O.setCircle(Grid::TCoord2D(0,0),3.0, rigid);
	draw(O,rgb,Nx,Ny);
	fprintf(stderr,"circle (-5,0) radius 3\n");
	O.setCircle(Grid::TCoord2D(-5,0),3.0, rigid);
	draw(O,rgb,Nx,Ny);
		
	fprintf(stderr,"Line (0,0)->(5,5)\n");
	O.setLine(0,0,5,5,rigid);
	draw(O,rgb,Nx,Ny);
	
	fprintf(stderr,"Line (5,5)->(9,0)\n");
	O.setLine(5,5,9,0,rigid);
	draw(O,rgb,Nx,Ny);
	
	fprintf(stderr,"Line (9,0)->(-5-5)\n");
	O.setLine(9,0,-5,-5,rigid);
	draw(O,rgb,Nx,Ny);
	
	fprintf(stderr,"Beam (10,0,PI/2) width PI/8\n");
	O.setBeam(Grid::TCoord2D(10,0),10,M_PI/2,M_PI/8,occu);
	draw(O,rgb,Nx,Ny);
	
	fprintf(stderr,"Arc on top of the beam\n");
	O.setArc(Grid::TCoord2D(10,0),10+0.5,M_PI/2,M_PI/8,rigid);
	draw(O,rgb,Nx,Ny);
}

void testScan(TOccupancyGrid &O, unsigned char *rgb, int Nx, int Ny){
	Grid::TScan s;
	float a=0;
	s.alloc(180);
	grid_t empty=0.0;
	grid_t rigid=254;
	grid_t occu=128;
	fprintf(stderr,"1\n");
	for(int i=0;i<180;i++){
		s.r[i] = 5.0 + 3.0*sin(a*4);
		s.a[i] = a;
		a+=(M_PI/180.0); 
		//fprintf(stderr,"(%.2f %.2f) ",s.r[i],s.a[i] ); 
	}
	

	O.setScanPoints(-10, 0 ,0, s, occu);
	draw(O,rgb,Nx,Ny);
	
	O.setScanFillSimple(0, 0 ,0, s,empty,rigid);
	draw(O,rgb,Nx,Ny);
	
	fprintf(stderr,"Testing probabilistic way\n");
	usleep(1000*1000);
	O.setGridToValue(0.5);
	O.fillRGB(rgb,1.0/255.0);
	displayImage(0, rgb,Ny,Nx);
	usleep(500*1000);
	
	for(int i=0;i<5;i++){
		fprintf(stderr,"Adding a scan\n");
		O.setScanFillProb(0, 0 ,0, s,0.4,0.6);
		O.fillRGB(rgb,1.0/255.0);
		displayImage(0, rgb,Ny,Nx);
		usleep(500*1000);
	}
	fprintf(stderr,"Testing adding scan with rigid structures\n");
	O.setGridToValue(64);
	O.setLine(-5,0,-5,5,rigid);
	O.setLine(-5,5,5,5,rigid);
	O.setLine(5,5,5,0,rigid);
	draw(O,rgb,Nx,Ny);
	
	O.setScanFillWithRigidWalls(0, 
															0,
															0, 
															s, 
															empty,
															occu,
															rigid);

}
void testGetScan(TOccupancyGrid &O, unsigned char *rgb, int Nx, int Ny){
	Grid::TScan s;
	float a=0;
	s.alloc(180);
	grid_t empty=0.0;
	grid_t rigid=254;
	grid_t occu=128;
	
	O.setGridToValue(64);
	O.setLine(-5,0,-5,5,rigid);
	O.setLine(-5,5,5,5,rigid);
	O.setLine(5,5,5,0,rigid);
	draw(O,rgb,Nx,Ny);
	
	///Get scan from (0,0,0)
	O.getScan(0,0, 0, 180, M_PI/180, occu, s);
	///Draw it to other place
	O.setScanFillSimple(11, 0 ,0, s,empty,occu);
	
	draw(O,rgb,Nx,Ny);
}

void testMCL(TOccupancyGrid &O, unsigned char *rgb, int Nx, int Ny){
	Grid::TScan s;
	float a=0;
	s.alloc(180);
	grid_t empty=0.0;
	grid_t rigid=254;
	grid_t occu=128;
	long int s1,us1,s2,us2;
		
	O.setGridToValue(64);
	O.setLine(-15,0,-15,15,rigid);
	O.setLine(-15,15,15,15,rigid);
	O.setLine(15,15,15,0,rigid);
	draw(O,rgb,Nx,Ny);
	
	fprintf(stderr,"Starting efficiency test... might take a while");
	ownTime_GetTimeOfDay(&s1, 
											 &us1);

	for(float x = -5; x<5;x+=0.1){
		for(float y=0; y<5; y+=0.1){
			O.getScan(x,y, 0, 180, M_PI/180, occu, s);
		}
	}
	ownTime_GetTimeOfDay(&s2, 
												&us2);

	long int sec = s2-s1;
	long int us = us2-us1;
	
	long int ms = sec*1000+us/1000;
	
	fprintf(stderr,"It took %ldms to calculate 100*50*180=%ldms/line scanlines - happy?\n",
					ms,
				 ms/(100*50*180));
}

void testDistanceTransform(TOccupancyGrid &O, unsigned char *rgb, int Nx, int Ny){
	Grid::TScan s;
	s.alloc(180);
	
	grid_t empty=0.0;
	grid_t rigid=254;
	grid_t occu=128;
	long int s1,us1,s2,us2;
	float *dist=(float *) malloc(Nx*Ny*sizeof(float)) ;	
	float scale;
	
	O.setGridToValue(empty);
	O.setLine(-15,0,-15,15,rigid);
	O.setLine(-15,15,15,15,rigid);
	O.setLine(15,15,15,0,rigid);
	draw(O,rgb,Nx,Ny);
	
	fprintf(stderr,"Starting the transform... ");
	ownTime_GetTimeOfDay(&s1, 
												&us1);

	float maxd = O.distanceTransform(dist,occu);
	ownTime_GetTimeOfDay(&s2, 
												&us2);

	long int sec = s2-s1;
	long int us = us2-us1;
	
	long int ms = sec*1000+us/1000;
	
	fprintf(stderr,"It took %ldms to calculate\n",ms);
	scale = 255.0/maxd;
	
	for(int j=0;j<Ny;j++){
		for(int i=0;i<Nx;i++){
			rgb[3*Ny*j+3*i] = (unsigned char) (dist[j*Ny+i]*scale);
			rgb[3*Ny*j+3*i+1] = (unsigned char) (dist[j*Ny+i]*scale);
			rgb[3*Ny*j+3*i+2] = (unsigned char) (dist[j*Ny+i]*scale);
		}
	}
	fprintf(stderr,"Max dist to obstacle seems to be %.3fm\n",maxd);
	displayImage(0, rgb,Ny,Nx);
	usleep(1000*1000);
	
	fprintf(stderr,"Starting efficiency test... might take a while");
	ownTime_GetTimeOfDay(&s1, 
												&us1);

	for(float x = -5; x<5;x+=0.1){
		for(float y=0; y<5; y+=0.1){
			O.getScan(x,y, 0, 180, M_PI/180, occu, dist, s);
		}
	}
	ownTime_GetTimeOfDay(&s2, 
												&us2);

	sec = s2-s1;
	us = us2-us1;
	
	ms = sec*1000+us/1000;
	
	fprintf(stderr,"It took %ldms to calculate 100*50*180=%.4fms/line scanlines - happy?\n",
					ms,
					(float)ms/(100.0*50.0*180.0));
	
	O.setGridToValue(64);
	O.setLine(-15,0,-15,15,rigid);
	O.setLine(-15,15,15,15,rigid);
	O.setLine(15,15,15,0,rigid);
	O.getScan(0,0, 0, 180, M_PI/180, occu, dist, s);
	O.setScanFillSimple(0, 0 ,0, s,empty,occu);
	draw(O,rgb,Nx,Ny);
	free(dist);
}

void testPlanning(TOccupancyGrid &O, unsigned char *rgb, int Nx, int Ny){
	std::vector<TPathPixel> path;
	float *dist=(float *) malloc(Nx*Ny*sizeof(float)) ;
	grid_t empty=0.0;
	grid_t rigid=254;
	grid_t occu=128;
	grid_t *data;
	fprintf(stderr,"Planning demo\n");
	fprintf(stderr,"This does shortest-safe-path-planning\n");
	
	fprintf(stderr,"Initializing to zero\n");
	O.setGridToValue(0);
	draw(O,rgb,Nx,Ny);
	fprintf(stderr,"Setting map primitives\n");
	O.setLine(-5,0,-5,5,255);
	O.setLine(-5,5,5,5,255);
	O.setLine(5,5,5,0,255);
	O.setLine(5,0,2,0,255);
	O.setCircle(Grid::TCoord2D(0,-4),1,255);
	O.setCircle(Grid::TCoord2D(-3,-2),1,255);
	O.setCircle(Grid::TCoord2D(6,-2),1,255);
	O.setCircle(Grid::TCoord2D(-6,-3),1,255);
	
	draw(O,rgb,Nx,Ny);
	fprintf(stderr,"Setting border to grid\n");
	data = O.getDataPtr();
	for(int i=0;i<Nx;i++){
		data[i] = 255;
		data[Nx*(Ny-1)+i] = 255;  	
	}
	for(int i=0;i<Ny;i++){
		data[i*Nx]=255;
		data[i*Nx+Nx-1]=255;
	}
	draw(O,rgb,Nx,Ny);
	
	fprintf(stderr,"Doing distance transform\n");
	float maxd = O.distanceTransform(dist,occu);
	float scale = 255.0/maxd;
	
	for(int j=0;j<Ny;j++){
		for(int i=0;i<Nx;i++){
			rgb[3*Ny*j+3*i] = (unsigned char) (dist[j*Ny+i]*scale);
			rgb[3*Ny*j+3*i+1] = (unsigned char) (dist[j*Ny+i]*scale);
			rgb[3*Ny*j+3*i+2] = (unsigned char) (dist[j*Ny+i]*scale);
		}
	}
	fprintf(stderr,"And here is the result\n");
	displayImage(0, rgb,Ny,Nx);
	usleep(1000*1000);
	
	fprintf(stderr,"'Inverting' the distance transform\n");
	for(int i=0;i<Nx*Ny;i++){
	  if(dist[i] < 0.5) dist[i] = 255;
		else{
			dist[i] = (maxd/dist[i]);
		}
	}
	float value;
	for(int j=0;j<Ny;j++){
		for(int i=0;i<Nx;i++){
			if(dist[j*Ny+i] != 255 ) value = dist[j*Ny+i]*(220/(maxd/0.5));
			else value = dist[j*Ny+i];
			rgb[3*Ny*j+3*i] = (unsigned char) (value);
			rgb[3*Ny*j+3*i+1] = (unsigned char) (value);
			rgb[3*Ny*j+3*i+2] = (unsigned char) (value);
		}
	}
	fprintf(stderr,"And here is the result\n");
	displayImage(0, rgb,Ny,Nx);
	usleep(100*1000);

	fprintf(stderr,"Now doing the path plan... Will take some time\n");
	CJanRoutePlanner planner(dist,Nx,Ny);
	
	//Grid::TPixel pix = O.toPixel(Grid::TCoord2D(4,2)); 
	Grid::TPixel pix = O.toPixel(Grid::TCoord2D(1,-8)); 
	TPathPixel start(pix.x,pix.y);
	//pix = O.toPixel(Grid::TCoord2D(2,8));
	pix = O.toPixel(Grid::TCoord2D(-2,3));
	TPathPixel end(pix.x,pix.y);		
	planner.plan(start,end,path);
	
	for(int i=0;i<path.size();i++){
		O.setValue(Grid::TPixel(path[i].x,path[i].y),255);
	}
	draw(O,rgb,Nx,Ny);
	
	usleep(5000*1000);
}

void testPlanning2(TOccupancyGrid &O, unsigned char *rgb, int Nx, int Ny){
	std::vector<TPathPixel> path;
	float *dist=(float *) malloc(Nx*Ny*sizeof(float)) ;
	float *gradient = (float *) malloc(Nx*Ny*sizeof(float)) ;
	grid_t empty=0.0;
	grid_t rigid=254;
	grid_t occu=128;
	grid_t *data;
	float scale;
	fprintf(stderr,"Planning demo\n");
	fprintf(stderr,"This does shortest-safe-path-planning for heightmap gradient\n");
	
	data = O.getDataPtr();
	float maxd=-10000 ;
	for(int j=0;j<O.size();j++){
		if(data[j]>maxd)maxd=data[j];
	}
	fprintf(stderr,"\n----------- Heightmap -------------\n");
	scale = 255.0/(maxd);
	for(int j=0;j<Ny;j++){
		for(int i=0;i<Nx;i++){
			rgb[3*Ny*j+3*i] = (unsigned char) (data[j*Ny+i]*scale);
			rgb[3*Ny*j+3*i+1] = (unsigned char) (data[j*Ny+i]*scale);
			rgb[3*Ny*j+3*i+2] = (unsigned char) (data[j*Ny+i]*scale);
		}
	}
	displayImage(0, rgb,Ny,Nx);
	usleep(5000*1000);
	
	
	
	maxd = O.gradient(gradient);
	for(int j=0;j<O.size();j++){
		data[j] = gradient[j];
	}
	fprintf(stderr,"Heightmap gradient\n");
	scale = 255.0/(maxd);
	
	for(int j=0;j<Ny;j++){
		for(int i=0;i<Nx;i++){
			rgb[3*Ny*j+3*i] = (unsigned char) (data[j*Ny+i]*scale);
			rgb[3*Ny*j+3*i+1] = (unsigned char) (data[j*Ny+i]*scale);
			rgb[3*Ny*j+3*i+2] = (unsigned char) (data[j*Ny+i]*scale);
		}
	}
	
	displayImage(0, rgb,Ny,Nx);
	usleep(5000*1000);
	for(int j=0;j<O.size();j++){
		data[j] = gradient[j];
	}

	maxd=0;
	float mind=10000;
	
	for(int i=0;i<Nx*Ny;i++){
		data[i] = fabs(data[i]);
		if(data[i]>1.0) data[i] = 255;
		//else if(data[i]>0.8) data[i] = 128;
		//if(data[i]>1.0) data[i] = 1;
		//else if(data[i]>0.5) data[i] = 1;
		//else{
		//		data[i]=0;
		//}
		if(data[i]>maxd) maxd=data[i];
		if(data[i]<mind) mind=data[i];
	}
	
	
	//O.save("binary.txt");
	
	fprintf(stderr,"Max=%f Min=%f\n",maxd,mind);
	scale = 255.0/(maxd);
	for(int j=0;j<Ny;j++){
		for(int i=0;i<Nx;i++){
			rgb[3*Ny*j+3*i] = (unsigned char) (data[j*Ny+i]);
			rgb[3*Ny*j+3*i+1] = (unsigned char) (data[j*Ny+i]);
			rgb[3*Ny*j+3*i+2] = (unsigned char) (data[j*Ny+i]);
		}
	}
	
	displayImage(0, rgb,Ny,Nx);
	usleep(5000*1000);

	/////////////////////////////////////////////////////////////////
	fprintf(stderr,"Doing distance transform\n");
	maxd = O.distanceTransform(dist,occu);
	scale = 255.0/maxd;
	
	for(int j=0;j<Ny;j++){
		for(int i=0;i<Nx;i++){
			rgb[3*Ny*j+3*i] = (unsigned char) (dist[j*Ny+i]*scale);
			rgb[3*Ny*j+3*i+1] = (unsigned char) (dist[j*Ny+i]*scale);
			rgb[3*Ny*j+3*i+2] = (unsigned char) (dist[j*Ny+i]*scale);
		}
	}
	fprintf(stderr,"And here is the result\n");
	displayImage(0, rgb,Ny,Nx);
	usleep(2000*1000);
	
	fprintf(stderr,"'Inverting' the distance transform\n");
	for(int i=0;i<Nx*Ny;i++){
		if(dist[i] < 0.5) dist[i] = 255;
		else{
			dist[i] = (maxd/dist[i]);
		}
	}
	float value;
	for(int j=0;j<Ny;j++){
		for(int i=0;i<Nx;i++){
			if(dist[j*Ny+i] != 255 ) value = dist[j*Ny+i]*(220/(maxd/0.5));
			else value = dist[j*Ny+i];
			rgb[3*Ny*j+3*i] = (unsigned char) (value);
			rgb[3*Ny*j+3*i+1] = (unsigned char) (value);
			rgb[3*Ny*j+3*i+2] = (unsigned char) (value);
		}
	}
	fprintf(stderr,"And here is the result\n");
	displayImage(0, rgb,Ny,Nx);
	usleep(100*1000);

	/////////////////////////////////////////////////////////////////
	
	
	fprintf(stderr,"Now doing the path plan... Will take some time\n");
	
	long int s1,us1,s2,us2;
	ownTime_GetTimeOfDay(&s1, 
												&us1);
	
	CJanRoutePlanner planner(dist,Nx,Ny);
	
	//Grid::TPixel pix = O.toPixel(Grid::TCoord2D(4,2)); 
	Grid::TPixel pix = O.toPixel(Grid::TCoord2D(2,8)); 
	TPathPixel start(pix.x,pix.y);
	//pix = O.toPixel(Grid::TCoord2D(2,8));
	pix = O.toPixel(Grid::TCoord2D(-4,0));
	TPathPixel end(pix.x,pix.y);		
	planner.plan(start,end,path);
	
	ownTime_GetTimeOfDay(&s2, 
												&us2);

	long int sec = s2-s1;
	long int us = us2-us1;
	
	long int ms = sec*1000+us/1000;
	sec = s2-s1;
	us = us2-us1;
	
	ms = sec*1000+us/1000;
	
	fprintf(stderr,"It took %ldms to calculate\n",ms);
	
	for(int i=0;i<path.size();i++){
		data[path[i].y*Nx+path[i].x]=255;
		//O.setValue(Grid::TPixel(path[i].x,path[i].y),255);
	}
	
	fprintf(stderr,"And here is the result N=%d\n",path.size());
	draw(O,rgb,Nx,Ny);
	
	fprintf(stderr,"\nEnd of Demo -- Press enter (in console) to stop!\n");
	char c;
	scanf("%c",&c);
	free(gradient);
	free(dist);
}


int main(void){
	TOccupancyGrid O(45.1,45.1,0.1,Grid::TCoord2D(0,0));
	grid_t empty=0.0;
	grid_t rigid=254;
	grid_t occu=128;
	int Nx,Ny;
	unsigned char *rgb;
	unsigned char *gray;
	float *data;
	
	CJPEGEncoder encoder;
	
	///Initialize grid as empty
	O.setGridToValue(empty);
	
	//For planning2
	//O.loadGModel("gradient.gmodel");
	O.loadGModel("ground.gmodel");
	
	O.getDim(Nx,Ny);
	rgb = (unsigned char *) malloc(3*Nx*Ny);
	//rgb = (unsigned char *) malloc(3*320*240);
	///Initialize Image display
	addImageWindow(Nx, Ny);
	glutImageGUI_Start();
	
	gray = (unsigned char *)malloc(Nx*Ny);
	
	draw(O,rgb,Nx,Ny);
	data = O.getDataPtr();
	for(int i=0;i<O.size();i++){
		gray[i]=(unsigned char) 100*fabs(data[i]);
	}
#if 0
	long int s1,us1,s2,us2;
	ownTime_GetTimeOfDay(&s1, 
												&us1);
	/*
	*/
	//ownTime_GetTimeOfDay(&s2, 
	//											&us2);

	encoder.SetGrayscaleDataPtr((char *) gray,Nx,Ny); 
	//encoder.SetRGBDataPtr((char *) rgb,Nx,Ny); 								
	if (encoder.GetJPEGDataPtr() != NULL){
		//fprintf(stderr,"Grayscale JPEG Compression SUCCESS\n");
		FILE *f = fopen("test.jpg","wb");		
		fwrite(encoder.GetJPEGDataPtr(),encoder.GetJPEGDataSize(),1,f);
		fclose(f);
	}
	else{
		//fprintf(stderr,"--------- Failed to compress --------\n");
	}
	ownTime_GetTimeOfDay(&s2, 
												&us2);

	long int sec = s2-s1;
	long int us = us2-us1;
	
	long int ms = sec*1000+us/1000;
	sec = s2-s1;
	us = us2-us1;
	
	ms = sec*1000+us/1000;
	float fms=(float)sec*1000.0+(float)us/1000.0;;
	fprintf(stderr,"It took %.4f ms to calculate\n",fms);
	for(int j=0;j<Ny;j++){
		for(int i=0;i<Nx;i++){
			rgb[3*Nx*j+3*i] = (unsigned char) (gray[j*Nx+i]);
			rgb[3*Nx*j+3*i+1] = (unsigned char) (gray[j*Nx+i]);
			rgb[3*Nx*j+3*i+2] = (unsigned char) (gray[j*Nx+i]);
		}
	}
	fprintf(stderr,"Titta på\n");
	displayImage(0, rgb,Ny,Nx);
	usleep(5000*1000);
	exit(1);
#endif
	
	//testPrimitives(O,rgb,Nx,Ny);
	//testScan(O,rgb,Nx,Ny);
	//testGetScan(O,rgb,Nx,Ny);
	//testMCL(O,rgb,Nx,Ny);
	//testDistanceTransform(O,rgb,Nx,Ny);
	//testPlanning(O,rgb,Nx,Ny);
	testPlanning2(O,rgb,Nx,Ny);
	while(1){
		O.fillRGB(rgb, empty, occu, rigid);		
		displayImage(0, rgb,Ny,Nx);
		usleep(500*1000);
	}
	
return 0;
}
